import rclpy
from rclpy.node import Node
from topic_msg.msg import CatVision
import serial
import random


class CatEyesNode(Node):
    MIN_X = MIN_Y = 0
    MAX_X = MAX_Y = 50
    MID_X = MID_Y = 25
    K_V = 0.07

    def __init__(self, name):
        super().__init__(name)
        self._current_x = self.MID_X
        self._current_y = self.MID_Y

        self._serial = serial.Serial("/dev/ttyACM0", 115200, timeout=1)
        self._cat_vision_subscriper = self.create_subscription(
            CatVision, "cat_vision", self._rcv_cat_vision_callback, 10
        )

    def __del__(self):
        self._serial.close()
        self._cat_vision_subscriper.destroy()

    def _rcv_cat_vision_callback(self, msg):
        if self._no_object(msg) is True:
            self._random_angle()
        else:
            self._set_angle(msg)

    def _no_object(self, msg):
        if msg.number == 0:
            return True
        else:
            if msg.number != len(msg.x) or msg.number != len(msg.y):
                return True
            else:
                return False

    def _random_angle(self):
        x = self.MID_X
        y = self.MID_Y
        i = random.randint(0, 100)
        if i % 10 == 1:
            x = random.randint(self.MIN_X + 20, self.MAX_X - 20)
            y = random.randint(self.MIN_Y + 20, self.MAX_Y - 20)

        self._set_position(x, y)

    def _set_angle(self, msg):
        viosion_x = 0
        viosion_y = 0
        for i in range(msg.number):
            viosion_x += msg.x[i]
            viosion_y += msg.y[i]

        viosion_x = int(viosion_x / msg.number)
        viosion_y = int(viosion_y / msg.number)

        err_x = viosion_x - int(msg.width / 2)
        err_y = viosion_y - int(msg.height / 2)

        x = int(self.MID_X + err_x * self.K_V)
        y = int(self.MID_Y - err_y * self.K_V)
        self._set_position(x, y)

    def _set_position(self, x, y):
        if x > self.MAX_X:
            x = self.MAX_X
        elif x < self.MIN_X:
            x = self.MIN_X

        if y > self.MAX_Y:
            y = self.MAX_Y
        elif y < self.MIN_X:
            y = self.MIN_Y

        if self._current_x == x and self._current_y == y:
            pass
        else:
            cmd = (
                ":"
                + format(x, "0{}d".format(2))
                + ","
                + format(y, "0{}d".format(2))
                + "."
            )
            self._current_x = x
            self._current_y = y
            self._serial.write(cmd.encode())


def main():
    rclpy.init()
    node = CatEyesNode("cat_eyes")
    rclpy.spin(node)
    rclpy.shutdown()
